#include "cartographer/rcm/dynamic/multi_dynamic_detect.h"

namespace cartographer
{
    MultiDynamicDetector::MultiDynamicDetector() : DynamicDetector()
    {
        auto &option = DynamicOptions::GetInstance();
        float orgScale = option.subpixelScale;
        float orgHitRatio = option.hitRatio;
        for (int i = option.dynScaleIterTimes; i > 0; i--)
        {
            double scale = orgScale + option.dynScaleStride * i;
            scales_.push_back(scale);
            LOG(INFO) << " add scale " << scale;
        }
        scales_.push_back(orgScale);
        for (int i = 1; i <= option.dynScaleIterTimes; i++)
        {
            double scale = orgScale - option.dynScaleStride * i;
            scales_.push_back(scale);
            LOG(INFO) << " add scale " << scale;
        }
        //(scale / orgScale)越大表示格子越多，格子越细，则hit概率越大
        for (auto scale : scales_)
        {
            float ratio = orgHitRatio * (scale / orgScale);
            hitRatios_.push_back(ratio);
            LOG(INFO) << " add scale " << scale << " scaleRatio " << (scale / orgScale) << " hitRatio " << ratio;
        }
    }

    DynGridMap::DynGridMap(const mapping::MapLimits &limitOrg, float pixelScale, float hitRatio) : limit(limitOrg)
    {
        const double resolutionOrg = limit.resolution();
        // pixelScale越小， 分辨率越大，则格子数量越少
        resolution = resolutionOrg / pixelScale;
        int maxIdxX = limit.cell_limits().num_x_cells;
        int maxIdxY = limit.cell_limits().num_y_cells;
        mapping::CellLimits cellLimits(maxIdxX * pixelScale, maxIdxY * pixelScale);
        mapping::MapLimits scaledLimits(resolution, limit.max(), cellLimits);
        hitRatio = hitRatio;
        limit = scaledLimits;
    }

    void DynGridMap::Update(const Eigen::Vector2f &point, bool uniqueHit)
    {
        int index = GetLinIndex(point);
        if (cells.find(index) == cells.end())
        {
            cells[index] = {1, 1, hitRatio * (float)1.0};
        }
        else
        {
            cells[index].hitCnt++;
            cells[index].probability *= hitRatio;
            if (uniqueHit)
                cells[index].uniqueHitCnt++;
        }
    }

    int DynGridMap::GetLinIndex(const Eigen::Vector2f &point)
    {
        auto cellIdx = limit.GetCellIndex(point);
        return GetLinearIndex(cellIdx, limit);
    }

    bool CompareRatio(std::pair<int, float> a, std::pair<int, float> b)
    {
        return a.second > b.second;
    }

    void MultiDynamicDetector::DetectDynPoint2(mapping::Grid2D *grid, const sensor::RangeData &range_data, const common::Time time,
                                               std::vector<int> &dynCnts)
    {
        auto *const pgrid = static_cast<mapping::ProbabilityGrid *>(grid);
        CHECK(pgrid != nullptr);
        auto &options = DynamicOptions::GetInstance();
        size_t cacheSize = (size_t)options.cacheSize;
        while (cloudQueue_.size() > cacheSize - 1)
            cloudQueue_.pop_front();
        if (cloudQueue_.size() < cacheSize - 1)
            return;

        LOG(INFO) << " DetectDyn1 ";
        // 计算原始栅格地图的limit
        auto orgMapLimit = GrowMapLimits(range_data, pgrid);
        // clouds are enough
        int iterTimes = (int)scales_.size();
        std::vector<DynGridMap::Ptr> multiHitMaps;
        std::vector<float> distances;
        // 遍历每个分辨率
        for (int i = 0; i < iterTimes; i++)
        {
            //  根据每个分辨率构建grid
            auto cntMap = std::make_shared<DynGridMap>(orgMapLimit, scales_[i], hitRatios_[i]);
            LOG(INFO) << " DetectDyn2 " << i;
            // 前4帧投到栅格中计数
            for (auto &cloud : cloudQueue_)
            {
                std::vector<int> picked;
                //  将每帧点云的每个点放到栅格中，统计计数
                for (const auto &hit : cloud.returns)
                {
                    auto coord2d = hit.position.head<2>();
                    auto offset = coord2d - cloud.origin.head<2>();
                    if (offset.norm() > options.dynDetectDistMax)
                        continue; // 距离雷达远的点不考虑
                    int index = cntMap->GetLinIndex(coord2d);
                    bool uniqueHit = std::find(picked.begin(), picked.end(), index) == picked.end();
                    if (uniqueHit)
                        picked.push_back(index);
                    cntMap->Update(coord2d, uniqueHit);
                }
            }
            //当前帧投到栅格中计数
            std::vector<int> picked;
            for (const auto &hit : range_data.returns)
            {
                auto coord2d = hit.position.head<2>();
                auto offset = coord2d - range_data.origin.head<2>();
                float distance = offset.norm();
                distances.push_back(distance);
                if (distance > options.dynDetectDistMax)
                    continue; // 距离雷达远的点不考虑
                int index = cntMap->GetLinIndex(coord2d);
                bool uniqueHit = std::find(picked.begin(), picked.end(), index) == picked.end();
                if (uniqueHit)
                    picked.push_back(index);
                cntMap->Update(coord2d, uniqueHit);
            }
            multiHitMaps.push_back(cntMap);
        }
        size_t pointSize = range_data.returns.size();
        std::vector<std::pair<int, float>> ranks;
        // 遍历当前帧每个点
        std::vector<Eigen::Vector2f> dynPoints;
        for (size_t i = 0; i < pointSize; i++)
        {
            if (distances[i] > options.dynDetectDistMax)
                continue;
            int motionTimes = 0;
            float probability = 0.0;
            auto coord2d = range_data.returns[i].position.head<2>();
            for (int p = 0; p < iterTimes; p++)
            {
                auto &cntMap = multiHitMaps[p];
                int index = cntMap->GetLinIndex(coord2d);
                const auto &cell = cntMap->GetCell(index);
                probability += cell.probability;
                if (cell.uniqueHitCnt < options.motionSize)
                    motionTimes++;
            }
            if (motionTimes <= options.dynValidMotionTimes)
                continue;
            dynPoints.push_back(coord2d);
            ranks.push_back(std::make_pair(int(i), probability));
        }
        std::sort(ranks.begin(), ranks.end(), CompareRatio);
        size_t dynSize = ranks.size();
        std::vector<std::pair<int, float>> bestDynPoints;
        for (size_t i = 0; i < dynSize; i++)
        {
            size_t clusterSize = 0;
            auto currPt = dynPoints[i];
            for (size_t p = 0; p < dynSize; p++)
            {
                if (i == p)
                    continue;
                auto nearPt = dynPoints[p];
                if ((currPt - nearPt).norm() < options.dynNearRadius)
                    clusterSize++;
            }
            if (clusterSize > 5)
                bestDynPoints.push_back(ranks[i]);
        }
        dynCnts.resize(pointSize, 0);
        for (const auto &dpt : bestDynPoints)
        {
            dynCnts[dpt.first] = 5;
        }
    }

    void MultiDynamicDetector::DetectDynPoint(mapping::Grid2D *grid, const sensor::RangeData &range_data, const common::Time time,
                                              std::vector<int> &dynCnts)
    {
        auto *const pgrid = static_cast<mapping::ProbabilityGrid *>(grid);
        CHECK(pgrid != nullptr);
        auto &options = DynamicOptions::GetInstance();
        size_t cacheSize = (size_t)options.cacheSize;
        while (cloudQueue_.size() >= cacheSize)
            cloudQueue_.pop_front();
        cloudQueue_.push_back(range_data);
        size_t queueSize = cloudQueue_.size();
        // 等待收集足够的数据
        if (queueSize < cacheSize)
            return;
        // 计算原始栅格地图的limit
        auto orgMapLimit = GrowMapLimits(range_data, pgrid);
        // clouds are enough
        auto iterTimes = scales_.size();
        std::vector<std::map<int, int>> multiHitMaps;
        std::vector<mapping::MapLimits> multiLimits;
        // 遍历每个分辨率
        for (size_t i = 0; i < iterTimes; i++)
        {
            std::map<int, int> cntMap;
            // 根据每个分辨率构建grid
            const auto scaledLimits = ScaleGridResolution(orgMapLimit, scales_[i]);
            multiLimits.push_back(scaledLimits);
            // 计算所有点云在当前分辨率对应的栅格地图中的统计计数
            for (auto &cloud : cloudQueue_)
            {
                std::map<int, int> picked;
                // 将每帧点云的每个点放到栅格中，统计计数
                for (const auto &hit : cloud.returns)
                {
                    auto hitPt = hit.position.head<2>();
                    auto offset = hitPt - cloud.origin.head<2>();
                    if (offset.norm() > options.dynDetectDistMax)
                    {
                        // 距离雷达远的点不考虑
                        continue;
                    }
                    auto cellIdx = scaledLimits.GetCellIndex(hitPt);
                    int index = GetLinearIndex(cellIdx, scaledLimits);
                    // 检查当前帧是否已有点投到该栅格
                    if (picked.find(index) == picked.end())
                    {
                        if (cntMap.find(index) == cntMap.end())
                            cntMap[index] = 1;
                        else
                            cntMap[index]++;
                        picked[index] = 1;
                    }
                    else
                        picked[index]++;
                }
            }
            multiHitMaps.push_back(cntMap);
        }
        std::vector<int> lastDynCnts;
        std::vector<Eigen::Vector2f> dynPoints;
        // 遍历每帧点云，找出疑似动态点
        for (size_t i = 0; i < queueSize; i++)
        {
            const auto &rangeTmp = cloudQueue_[i];
            for (const auto &hit : rangeTmp.returns)
            {
                auto coord2d = hit.position.head<2>();
                auto offset = coord2d - rangeTmp.origin.head<2>();
                if (offset.norm() > options.dynDetectDistMax)
                {
                    // 太远的点不稳定，跳过
                    if (i == (queueSize - 1))
                        lastDynCnts.push_back(-1);
                    continue;
                }
                int motionTimes = 0;
                for (size_t p = 0; p < iterTimes; p++)
                {
                    auto &cntMap = multiHitMaps[p];
                    const auto &scaledLimits = multiLimits[p];
                    auto cellIdx = scaledLimits.GetCellIndex(coord2d);
                    int index = GetLinearIndex(cellIdx, scaledLimits);
                    if (cntMap[index] < options.motionSize)
                        motionTimes++;
                }
                // 最后一帧与前面的帧分开来
                if (i == (queueSize - 1))
                    lastDynCnts.push_back(motionTimes);
                else
                {
                    if (motionTimes > options.dynValidMotionTimes)
                        dynPoints.push_back(coord2d);
                }
            }
        }
        PickDynPoints(lastDynCnts, dynCnts, range_data);
        // PickDynPoints(lastDynCnts, dynCnts, range_data, dynPoints);
    }

    void MultiDynamicDetector::PickDynPoints(std::vector<int> &lastDynCnts, std::vector<int> &dynCnts, const sensor::RangeData &range_data)
    {
        auto &options = DynamicOptions::GetInstance();
        auto pointSize = range_data.returns.size();
        for (size_t i = 0; i < pointSize; i++)
        {
            // if (lastDynCnts[i] < 0 || lastDynCnts[i] <= options.dynValidMotionTimes)
            // {
            //     dynCnts.push_back(-1);
            //     continue;
            // }
            int nearStaticCnt = 0;
            int nearDynamicCnt = 0;
            auto currPt = range_data.returns[i].position.head<2>();
            for (size_t p = 0; p < pointSize; p++)
            {
                if (i == p)
                    continue;
                // if (i == p || lastDynCnts[p] < 0 || lastDynCnts[p] <= options.dynValidMotionTimes)
                //     continue;
                auto nearPt = range_data.returns[p].position.head<2>();
                if ((currPt - nearPt).norm() > options.dynNearRadius)
                    continue;
                if (lastDynCnts[p] < 0 || lastDynCnts[p] <= options.dynValidMotionTimes)
                    nearStaticCnt++;
                else
                    nearDynamicCnt++;
            }
            if (lastDynCnts[i] < 0 || lastDynCnts[i] <= options.dynValidMotionTimes)
            {
                if (nearDynamicCnt > nearStaticCnt * 3.0)
                    dynCnts.push_back(-2);
                else
                    dynCnts.push_back(-1);
            }
            else
            {
                if (nearDynamicCnt > 5)
                    dynCnts.push_back(5);
                else
                    dynCnts.push_back(-1);
            }
            // if (nearCnt > 5)
            //     dynCnts.push_back(5);
            // else
            //     dynCnts.push_back(-1);
        }
    }

    void MultiDynamicDetector::PickDynPoints(std::vector<int> &lastDynCnts, std::vector<int> &dynCnts, const sensor::RangeData &range_data,
                                             std::vector<Eigen::Vector2f> &dynPoints)
    {
        auto &options = DynamicOptions::GetInstance();
        for (size_t i = 0; i < lastDynCnts.size(); i++)
        {
            if (lastDynCnts[i] < 0)
            {
                dynCnts.push_back(-1);
                continue;
            }
            int nearCntDyn = 0;
            Eigen::Vector2f center;
            auto currPt = range_data.returns[i].position.head<2>();
            for (const auto &dynPt : dynPoints)
            {
                float dist = (currPt - dynPt).norm();
                if (dist < options.dynNearRadius)
                {
                    center += dynPt;
                    nearCntDyn++;
                }
            }
            center = center / (float)nearCntDyn;
            float dist2 = (currPt - center).norm();
            if (lastDynCnts[i] > options.dynValidMotionTimes)
            {
                // 候选动态点条件放宽
                if (nearCntDyn > options.dynNearNum || dist2 < (options.dynNearRadius * 0.7))
                {
                    dynCnts.push_back(lastDynCnts[i]);
                }
                else
                {
                    dynCnts.push_back(-4);
                }
            }
            else
            {
                // 非候选动态点条件放严
                if (nearCntDyn > options.dynNearNum * 2 && dist2 < options.dynNearRadius * 0.35)
                {
                    dynCnts.push_back(-2);
                }
                else
                {
                    dynCnts.push_back(-3);
                }
            }
        }
        // 如果非候选动态点被动态点夹住，则其很可能也是动态点的一部分
        for (size_t i = 0; i < dynCnts.size(); i++)
        {
            if (dynCnts[i] < 0 && dynCnts[i] != -2)
            {
                int preCnt = 0;
                int netCnt = 0;
                for (int f = 1; f < 6; f++)
                {
                    int pre = i - f;
                    int net = i + f;
                    if (pre < 0 || net > int(dynCnts.size() - 1))
                        continue;
                    if (dynCnts[pre] > options.dynValidMotionTimes)
                        preCnt++;
                    if (dynCnts[net] > options.dynValidMotionTimes)
                        netCnt++;
                }
                if (preCnt > 1 && netCnt > 1)
                {
                    dynCnts[i] = -2;
                }
            }
        }
    }
} // namespace cartographer